import numpy as np
from scipy import optimize

class TrajFilter:
    def __init__(self, lateralTraj, longTraj):
        self.lateralTraj = lateralTraj
        self.longTraj = longTraj

    def run(self):
        for index in range(len(self.longTraj) - 1, -1, -1):
            temp_f = self.longTraj[index]["f"]
            end_t = self.longTraj[index]["var"]

            vf = np.polyder(temp_f)
            vmin = vf(optimize.fminbound(vf, 0, end_t))
            vmax = vf(optimize.fminbound(lambda x: -vf(x), 0, end_t))
            af = np.polyder(temp_f, 2)
            amin = af(optimize.fminbound(af, 0, end_t))
            amax = af(optimize.fminbound(lambda x: -af(x), 0, end_t))
            jerkf = np.polyder(temp_f, 3)
            jerkmin = jerkf(optimize.fminbound(jerkf, 0, end_t))
            jerkmax = jerkf(optimize.fminbound(lambda x: -jerkf(x), 0, end_t))

            if (vmin < -1 or vmax > 10
                    or amin < -4.5 or amax > 4
                    or jerkmin < -4 or jerkmax > 2):
                self.longTraj[index].pop(index)